#include <stdlib.h>
#include "Car.h"
#include "zh_info_tool.h"
#include "mpu6050.h"
#include "ITM.h"
#include "bsp_uart.h"
#include "task_main.h"
#include "ws_config.h"
#include "Debug.h"
#include "Car_port.h"
#include "Car_ctrl.h"

Car_t car =
{
    .mode = CARMode_Stop,
    .pML = &MotorL,
    .pMR = &MotorR,
	.PID_P = 110,
	.PID_I = 0,
	.PID_D = 0,
	.baseSpeed = 600,
	.DisplayEN = 1,
	.XCnt = 0,
};

Tracker_t  traceData;

kalman1_t PowerADCFilter = Kalman1_init(1.5/3.3*4096,0,0.00001,0.1);
void Car_Init(void)
{
    Motor_Init();
    Car_Start();
	HAL_ADC_Start_DMA(&hadc1,(uint32_t*)&car.powerADC,1);
	Send_Panel();
}

void Car_Start(void)
{
    MotorL_Start();
    MotorR_Start();
#if SERVO_ENABLE == 1
	servo.start(&htim1, TIM_CHANNEL_1, 360);
    servo.writeAngle(180);
#endif
}

void Car_Set_Rate(int rateL, int rateR)
{
#if MotorPIDSpeedEnable == 1
	if(MotorL.pidEN == 1 && MotorR.pidEN == 1)
	{
		// Car_Set_Speed(rateL,rateR);
		car_ctrl_set_speed(rateL,rateR);
	}
#endif
	MotorL_Set_Rate(rateL);
	MotorR_Set_Rate(rateR);
	
}

void Car_Brake(void)
{
#if MotorPIDSpeedEnable == 1
	if(MotorL.pidEN == 1 && MotorR.pidEN == 1)
	{
		Car_Set_Speed(0,0);
	}
#endif 
    MotorL_Set_Rate(0);
    MotorR_Set_Rate(0);
    MotorL_Brake();
    MotorR_Brake();
}

void Car_Stop(void)
{
    MotorL_Stop();
    MotorR_Stop();
}

/**
 * @brief 设定闭环速度
 * @param speedL 左轮转速(脉冲数)
 * @param speedR 右轮转速(脉冲数)
 * @date 2022-11-24
 */
void Car_Set_Speed(int speedL, int speedR)
{
#if MotorPIDSpeedEnable == 1
	MotorL.speed = speedL;
	MotorR.speed = speedR;
#endif
}


void Car_Patrol_Start(void)
{
	car.mode = CARMode_Patrol;
	TraceBoardCtrl(1);//使能循迹
	if(car.XTraceEN == 1)RGB_SET(0,0,0,255);//蓝色
	else RGB_SET(0,255,0,255);//紫色
	car.DisplayEN = 0;//关闭显示提高性能
	bsp_MesureTimeStart(&car.PatrolTimeCounter);

}
void Car_Patrol_Stop(void)
{
	if(CARMode_Patrol == car.mode)
		car.mode = CARMode_Stop;
	TraceBoardCtrl(0);//关循迹
	Car_Brake();
	RGB_SET(0,0,0,0);
	car.PatrolTime = bsp_MesureTime(&car.PatrolTimeCounter);
	car.DisplayEN = 1;

}
void Car_CheckPower(void)
{
    car.power = kalman1_filter(&PowerADCFilter,car.powerADC);//电压滤波
	car.powerV = car.power * 0.009379f;
    /* 电池电量低处理*/
    if(car.powerV < 10.6)
    {
        LED_State = LED_NOPOWER_LOW;
       Beep(1000);
    }

}

void Car_Patrol(void)
{
	int Left_PWM=0,Right_PWM=0;
	int8_t track_Position_r,track_Position_l;

	if (car.mode != CARMode_Patrol) //   循迹模式
	{
		return;
	}

	//  获取位置偏移
	traceData.position = (int8_t)traceData.errR; //  右开始
	if (abs(traceData.position) <= 15)
	{
		track_Position_r = traceData.position;
	}

	traceData.position = (int8_t)traceData.errL; //  左开始
	if (abs(traceData.position) <= 15)
	{
		track_Position_l = traceData.position;
	}

	if (track_Position_l != track_Position_r) // 两条线
	{
		// WS_Borad_Open_Buzzer_Time(50);
	}

	//  进入循迹位置调整
	int16_t err_pwn_kp, err_pwn_kd;

	traceData.position = track_Position_r;

	if (traceData.data == 0) //  无黑线
	{
		// if(position < )
		traceData.position = traceData._position*2;//  用上次的值，放大2倍
	}
	else
	{
		traceData._position = traceData.position;//  保留上次位置值
		if (traceData.position > 4 || traceData.position < -4)
		{
			traceData.position *= 2;
		}
	}

	err_pwn_kp = (traceData.position) * (uint16_t)car.PID_P;
	err_pwn_kd = -mpu6050.Gyro_Z * (uint16_t)car.PID_D; //  微分抑制

	Right_PWM = car.baseSpeed - err_pwn_kp - err_pwn_kd ;
	Left_PWM = car.baseSpeed + err_pwn_kp + err_pwn_kd ;
	if (Left_PWM < -2400)
		Left_PWM = -2400;
	if (Right_PWM < -2400)
		Right_PWM = -2400;
	Car_Set_Rate(Left_PWM, Right_PWM);
}

char CarSt_Buf[10] = {0};//解析器的缓存,Split分割后的数据的引索存储到这里
//创建全局的St对象并初始化
Sentence_t CarSt = StCreate("car:", '/', ";", CarSt_Buf, NULL);
void CarCtrl_CMD(uint8_t *pdata, int len)
{
	CarSt.pSrcAdr = pdata;
	CarSt.SrcAdrSize = len;
	if(St_SPLIT(&CarSt) == 0)
	{
		return;
	}
	Beep(50);
	uint8_t* pstr = St_GetHex(&CarSt,0);
	char* ptemp;
	int temp1;
	int temp2;
	switch (*pstr)
	{

	case 'B'://设置基础速度
		car.baseSpeed = St_GetInt(&CarSt, 1);//car:B/3000;
		CarCtrl.fb_trackSpeed = car.baseSpeed;
		CarDebugf("[set] baseSpeed(%d)\n\r", car.baseSpeed);
		break;
	case 'C': // config
		if(St_CmpStr(&CarSt, 0, "C2"))//设置PID参数
		{
			temp1 = St_GetInt(&CarSt, 1);
			temp2 = St_GetInt(&CarSt, 2);
			car_task2_config_C(temp1, temp2);
			CarDebugf("time:%d, speed:%d\n\r",temp1, temp2);
		}
		else if(St_CmpStr(&CarSt, 0, "C3"))//设置PID参数
		{
			temp1 = St_GetInt(&CarSt, 1);
			temp2 = St_GetInt(&CarSt, 2);
			car_task3_config_B(temp1, temp2);
			CarDebugf("time:%d, speed:%d\n\r",temp1, temp2);
		}
		else if(St_CmpStr(&CarSt, 0, "C4"))//设置PID参数
		{
			temp1 = St_GetInt(&CarSt, 1);
			temp2 = St_GetInt(&CarSt, 2);
			int temp3 = St_GetInt(&CarSt, 3);
			int temp4 = St_GetInt(&CarSt, 4);
			int temp5 = St_GetInt(&CarSt, 5);
			car_task4_config_AB(temp1, temp2, temp3, temp4, temp5);
			CarDebugf("AllSpeed%d,time1:%d, speed1:%d, time2:%d, speed2:%d\n\r",
			          temp1, temp2, temp3, temp4, temp5);
		}
		break;
	case 'E':
		if(St_CmpStr(&CarSt, 0, "EV"))//使能速度闭环
		{
			temp1 = St_GetInt(&CarSt, 1); // car:EV/x;
			if (temp1 != 0)
			{
				MotorL.pidEN = 1;
				MotorR.pidEN = 1;
				CarCtrl.speed_fb_flag = 1;
			}
			else
			{
				CarCtrl.speed_fb_flag = 0;
				MotorL.pidEN = 0;
				MotorR.pidEN = 0;
			}
			CarDebugf("[set] Speed pidEN:(%d,%d)\n\r",MotorL.pidEN,MotorR.pidEN);
		}
		
		else if(St_CmpStr(&CarSt, 0, "EC"))
		{
			car.XCnt = 0;
			MotorL.encoder = 0;
			MotorR.encoder = 0;
			CarDebug("[log] XTrace Clear Cnt\n\r");
		}
		break;
	case 'V'://同时设置两轮
		ptemp = St_GetStr(&CarSt, 1);
		if(*ptemp == 'X' || *ptemp == 'L')
		{
			while(ptemp){
				if(*ptemp == 'Y' || *ptemp == 'R') break;
				ptemp++;
			}
			Car_Set_Rate(atoi(St_GetStr(&CarSt, 1)+1),atoi(ptemp+1));//car:V/X2000Y3000;
		}
		else
			Car_Set_Rate(St_GetInt(&CarSt, 1),St_GetInt(&CarSt, 2));//car:V/2000/3000;
		CarDebugf("[set] Speed(%d,%d)\n\r", MotorL.rate, MotorR.rate);

		break;
	case 'S'://刹车
		Car_Brake();//Car:S;
#if SERVO_ENABLE == 1
		servo.writeAngle(180);
#endif
		break;
	case 'T': //Trace
		// if(car.mode != CARMode_Patrol)
		// {
		// 	Car_Patrol_Start();
		// 	car_ctrl_start_track(car.baseSpeed);
		// 	// car_ctrl_start_rotate(CarCtrl.fb_angle, CarCtrl.fb_angleSpeed);
		// 	// car_ctrl_start_line(car.baseSpeed);
		// 	CarDebug("[log] Patrol Start\n\r");
		// }
		// else
		// {
		// 	Car_Patrol_Stop();
		// 	car_ctrl_stop();
		// 	CarDebugf("[log]Patrol\n\rTime    :%.2f s\n\r", (float)car.PatrolTime/1E3);
		// 	// CarDebugf("Distance:%.1f r\n\rSpeed   :%.1f r/min\n\r",\
		// 	// (float)car.PatrolX/520,(float)(car.PatrolX*1E3)/(car.PatrolTime*520)*60);
		// }
		car_ctrl_stop();
		break;
	case 't':
		if(St_CmpStr(&CarSt, 0, "task"))
		{
			temp1 = St_GetInt(&CarSt, 1);//car:task/x;
			if (temp1 == 1)
			{
				car_ctrl_task_start(CAR_CTRL_TASK1);
			}
			else if (temp1 == 2)
			{
				car_ctrl_task_start(CAR_CTRL_TASK2);
			}
			else if (temp1 == 3)
			{
			    car_ctrl_task_start(CAR_CTRL_TASK3);
			}
			else if (temp1 == 4)
			{
				car_ctrl_task_start(CAR_CTRL_TASK4);
			}
			CarDebugf("[set] Task %d ready \n\r",temp1);
		}
		else if (St_CmpStr(&CarSt, 0, "turn"))
		{
			car_ctrl_rotate_open(St_GetDob(&CarSt, 1), St_GetDob(&CarSt, 2));
		}
		break;
	case 'M':
		
		if(St_CmpStr(&CarSt, 0, "MPID"))//car:MPID,p,i,d;
		{
			MotorR.PID.Kp = St_GetInt(&CarSt, 1);
			MotorR.PID.Ki = St_GetInt(&CarSt, 2);
			MotorR.PID.Kd = St_GetInt(&CarSt, 3);
			MotorL.PID.Kp = MotorR.PID.Kp;
			MotorL.PID.Ki = MotorR.PID.Ki;
			MotorL.PID.Kd = MotorR.PID.Kd;
		}
		break;
	}
	if(St_CmpStr(&CarSt, 0, "PID"))//car:PID,20,1,1;
	{
//		car.PID_P = St_GetInt(&CarSt, 1);
		// car.PID_I = St_GetInt(&CarSt, 2);
//		car.PID_D = St_GetInt(&CarSt, 2);
		if (CarCtrl.track_fb_flag)
		{
			CarCtrl.pid_Track.Kp = St_GetDob(&CarSt, 1);
			CarCtrl.pid_Track.Kd = St_GetDob(&CarSt, 2);
			CarDebugf("[set] P(%.4f)D(%.4f)\n\r",
			CarCtrl.pid_Track.Kp, CarCtrl.pid_Track.Kd);
		}
		if (CarCtrl.angle_fb_flag)
		{
			CarCtrl.pid_Angle.Kp = St_GetDob(&CarSt, 1);
			CarCtrl.pid_Angle.Kd = St_GetDob(&CarSt, 2);
			CarCtrl.pid_Angle.Ki = St_GetDob(&CarSt, 3);
			CarCtrl.fb_angleSpeed = St_GetDob(&CarSt, 4);
			CarCtrl.fb_angle = St_GetDob(&CarSt, 5);
			CarDebugf("[set] P(%.4f)I(%.4f)D(%.4f)V(%.1f)A(%.1f)\n\r",
			CarCtrl.pid_Angle.Kp, CarCtrl.pid_Angle.Ki, CarCtrl.pid_Angle.Kd,
			CarCtrl.fb_angleSpeed, CarCtrl.fb_angle);
		}

	}


}


void Send_Panel(void)
{
///////////// Build panel in app

DEBUG("*.kwl");
DEBUG("clear_panel();");
DEBUG("set_grid_size(16,7);");
DEBUG("add_text(14,1,medium,C,SAVE,245,240,245,);");
DEBUG("add_text(13,0,medium,C,VPID,245,240,245,);");
DEBUG("add_button(11,2,14,car:Stop;,);");
DEBUG("add_button(12,2,20,car:EC;,);");
DEBUG("add_button(15,1,3,car:ER;,);");
DEBUG("add_switch(14,5,4,car:Trace;,car:Trace;,0,0);");
DEBUG("add_switch(14,0,2,car:EV/1;,car:EV/0;,0,0);");
DEBUG("add_slider(3,6,8,0,100,30,car:B/,;,1);");
DEBUG("add_slider(12,4,1,0,360,180,car:Angle/,;,0);");
DEBUG("add_4way_pad(0,4,X5000Y5000,X5000Y-5000,X-5000Y-5000,X-5000Y5000,X0Y0,0,car:V/,;);");
DEBUG("add_monitor(3,1,8,,3);");
DEBUG("add_send_box(11,6,3,40,car:B/,;);");
DEBUG("add_send_box(11,3,5,PID/1/0/0/0,car:,;);");
DEBUG("add_send_box(13,2,3,task/1,car:,;);");
DEBUG("set_panel_notes(SmartCarPanel,byLZH,From5524,);");
DEBUG("run();");
DEBUG("*");

}


//  寻找位置数据,   自右往左找
int8_t WS_Track_Find_Position_Right(uint16_t dat) //  1 2 3 4 5 6 7  8  9 10 11 12 13 14 15 16
{
	int l = 16, r = 16, i, strat = 0;
	int8_t position = 0;
	for (i = 0; i < 16; i++) //  自右往左找
	{
		if (strat == 1)
		{
			if ((dat & 1 << i) == 0) //  找变白的地点
			{
				r = i - 1;
				break;
			}
			if (i == 15)
			{
				r = i;
				break;
			}
		}
		if (strat == 0 && (dat & 1 << i) != 0) //  找变黑地点
		{
			if (i == 15) //  最后一个点
			{
				r = l = i;
				break;
			}
			else
			{
				l = i;
				strat = 1;
			}
		}
	}

	if (l < 16 && r < 16)
	{
		position = 15 - (l + r);
	}
	return position;
}

//  寻找位置数据,   自左往右找
int8_t WS_Track_Find_Position_Left(uint16_t dat) //  1 2 3 4 5 6 7  8  9 10 11 12 13 14 15
{
	int l = 16, r = 16, i, strat = 0;
	int8_t position = 0;
	for (i = 15; i >= 0; i--) //  自左往右找
	{
		if (strat == 1)
		{
			if ((dat & 1 << i) == 0) //  找变白的地点
			{
				r = i + 1;
				break;
			}
			if (i == 0)
			{
				r = i;
				break;
			}
		}
		if (strat == 0 && (dat & 1 << i) != 0) //  找变黑地点
		{
			if (i == 0)
			{
				r = l = i;
				break;
			}
			else
			{
				l = i;
				strat = 1;
			}
		}
	}

	if (l < 16 && r < 16)
	{
		position = 15 - (l + r);
	}
	return position;
}
uint8_t reverseBits(uint8_t input) {
    uint8_t output = 0;
    for (int i = 0; i < 8; i++) {
        output |= (input & 1) << (7 - i);
        input >>= 1;
    }
    return output;
}
//  读取循迹管数据
uint16_t  WS_Car_Get_Track_Date(void)
{
	uint16_t trackData = 0 ;
	int  i ;
	HAL_GPIO_WritePin(HC165_LD_GPIO_Port,HC165_LD_Pin,GPIO_PIN_RESET);
	i = 20 ;
	while( --i > 0);
	HAL_GPIO_WritePin(HC165_LD_GPIO_Port,HC165_LD_Pin,GPIO_PIN_SET);
	
	__NOP() ; __NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;
	//__NOP() ; __NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;

	for (i = 15 ; i >= 0 ; i -- )
	{
		if(HAL_GPIO_ReadPin(HC165_SER_GPIO_Port,HC165_SER_Pin) == GPIO_PIN_RESET)
		{
			trackData |= 1<<i;
		}
		HAL_GPIO_WritePin(HC165_CLK_GPIO_Port,HC165_CLK_Pin,GPIO_PIN_RESET);
		__NOP() ; __NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;
		__NOP() ; __NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;

		HAL_GPIO_WritePin(HC165_CLK_GPIO_Port,HC165_CLK_Pin,GPIO_PIN_SET);
		__NOP() ; __NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;
		// __NOP() ; __NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;__NOP() ;
	}
	// FIXME: 屏蔽两边的传感器
	trackData &= 0x7FFE;
	// 反转高八位
	// trackData = (reverseBits(trackData >> 8)<<8) | (trackData&0xFF);
	// 调换5,6位
	// tem = trackData 
	traceData.data = trackData;
	traceData.errR = WS_Track_Find_Position_Right(trackData);
	traceData.errL = WS_Track_Find_Position_Left(trackData);
	return trackData ;
}


